// fall_detection.h
#pragma once
#include <stdint.h>
#include <stdbool.h>

// 阈值定义
#define FALL_ACCEL_THRESHOLD      2.5f    // 加速度阈值（g）
#define FALL_GYRO_THRESHOLD       200.0f   // 角速度阈值（度/秒）
#define IMPACT_TIME_THRESHOLD     500     // 冲击时间阈值（ms）
#define STABLE_TIME_THRESHOLD     2000    // 稳定时间阈值（ms）

// MPU6050数据结构
typedef struct {
    float accel_x;    // 加速度 X（g）
    float accel_y;    // 加速度 Y（g）
    float accel_z;    // 加速度 Z（g）
    float gyro_x;     // 角速度 X（度/秒）
    float gyro_y;     // 角速度 Y（度/秒）
    float gyro_z;     // 角速度 Z（度/秒）
    long pts;          // 时间戳
} MPU6050_Data;

// 跌倒检测状态
typedef enum {
    FALL_STATE_NORMAL,        // 正常状态
    FALL_STATE_IMPACT,        // 冲击状态
    FALL_STATE_FALLING,       // 跌倒过程
    FALL_STATE_LYING         // 躺卧状态
} FallState;

// 跌倒检测结构体
typedef struct {
    FallState state;          // 当前状态
    uint32_t impact_time;     // 冲击开始时间
    uint32_t stable_time;     // 稳定开始时间
    float total_acc;          // 总加速度
    float total_gyro;         // 总角速度
    bool fall_detected;       // 跌倒检测标志
} FallDetection;

// fall_detection.c
#include "fall_detection.h"
#include <math.h>



// 获取当前时间的 PTS
long get_current_pts() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec * 1000) + (tv.tv_usec / 1000); // 转换为毫秒
}


//打pts 方法
data.pts = get_current_pts(); // 获取当前时间戳


// 初始化跌倒检测
void fall_detection_init(FallDetection* fall) {
    fall->state = FALL_STATE_NORMAL;
    fall->impact_time = 0;
    fall->stable_time = 0;
    fall->total_acc = 0;
    fall->total_gyro = 0;
    fall->fall_detected = false;
}

// 计算三轴合加速度
static float calculate_total_acceleration(const MPU6050_Data* data) {
    return sqrtf(data->accel_x * data->accel_x + 
                 data->accel_y * data->accel_y + 
                 data->accel_z * data->accel_z);
}

// 计算三轴合角速度
static float calculate_total_gyro(const MPU6050_Data* data) {
    return sqrtf(data->gyro_x * data->gyro_x + 
                 data->gyro_y * data->gyro_y + 
                 data->gyro_z * data->gyro_z);
}

// 检测是否处于躺卧状态
static bool is_lying_position(const MPU6050_Data* data) {
    // 检查Z轴加速度是否接近水平
    return fabsf(data->accel_z) < 0.5f;
}

// 跌倒检测主函数
bool fall_detection_update(FallDetection* fall, const MPU6050_Data* data, uint32_t current_time) {
    float total_acc = calculate_total_acceleration(data);
    float total_gyro = calculate_total_gyro(data);
    
    switch(fall->state) {
        case FALL_STATE_NORMAL:
            // 检测冲击
            if (total_acc > FALL_ACCEL_THRESHOLD || total_gyro > FALL_GYRO_THRESHOLD) {
                fall->state = FALL_STATE_IMPACT;
                fall->impact_time = current_time;
                fall->total_acc = total_acc;
                fall->total_gyro = total_gyro;
            }
            break;
            
        case FALL_STATE_IMPACT:
            // 检查冲击持续时间
            if (current_time - fall->impact_time > IMPACT_TIME_THRESHOLD) {
                if (is_lying_position(data)) {
                    fall->state = FALL_STATE_FALLING;
                    fall->stable_time = current_time;
                } else {
                    fall->state = FALL_STATE_NORMAL;
                }
            }
            break;
            
        case FALL_STATE_FALLING:
            // 检查是否持续躺卧
            if (is_lying_position(data)) {
                if (current_time - fall->stable_time > STABLE_TIME_THRESHOLD) {
                    fall->state = FALL_STATE_LYING;
                    fall->fall_detected = true;
                }
            } else {
                fall->state = FALL_STATE_NORMAL;
            }
            break;
            
        case FALL_STATE_LYING:
            // 检查是否恢复正常姿态
            if (!is_lying_position(data)) {
                fall->state = FALL_STATE_NORMAL;
                fall->fall_detected = false;
            }
            break;
    }
    
    return fall->fall_detected;
}

// mpu6050.h
#pragma once
#include <stdint.h>

// MPU6050寄存器地址定义
#define MPU6050_ADDR            0x68
#define MPU6050_ACCEL_XOUT_H   0x3B
#define MPU6050_GYRO_XOUT_H    0x43
#define MPU6050_PWR_MGMT_1     0x6B

// 初始化函数声明
void mpu6050_init(void);
void mpu6050_read_data(MPU6050_Data* data);


// main.c
#include "fall_detection.h"
#include "mpu6050.h"
#include <stdio.h>

// 获取系统时间（需要根据具体平台实现）
uint32_t get_system_time_ms(void);

int main(void) {
    // 初始化MPU6050
    mpu6050_init();
    
    // 初始化跌倒检测
    FallDetection fall_detector;
    fall_detection_init(&fall_detector);
    
    MPU6050_Data sensor_data;
    
    while(1) {
        // 读取传感器数据
        mpu6050_read_data(&sensor_data);

        //打印
        //printf("IMU Data: X: %.2f, Y: %.2f, Z: %.2f, PTS: %ld ms\n", data.x, data.y, data.z, data.pts);
        
        // 更新跌倒检测
        uint32_t current_time = get_system_time_ms();
        bool fall_detected = fall_detection_update(&fall_detector, &sensor_data, current_time);
        
        // 检测到跌倒
        if (fall_detected) {
            printf("Fall detected!\n");
            // 在这里添加报警处理代码
            // 例如：发送警报、蜂鸣器响警等
        }
        
        // 适当的延时
        delay_ms(10);
    }
    
    return 0;
}

/*
关键参数调整建议：
FALL_ACCEL_THRESHOLD：根据实际测试调整，通常在2-3g之间
FALL_GYRO_THRESHOLD：根据实际测试调整，通常在150-250度/秒之间
IMPACT_TIME_THRESHOLD：冲击检测时间，建议300-700ms
STABLE_TIME_THRESHOLD：躺卧确认时间，建议1-3秒
优化建议：
添加数字滤波器去除噪声
实现自适应阈值
添加姿态解算提高准确性
考虑添加温度补偿
实现防误报机制

*/

// 添加滤波器
static void apply_filter(MPU6050_Data* data) {
    // 简单的移动平均滤波示例
    static MPU6050_Data history[FILTER_SIZE];
    static int index = 0;
    
    // 保存历史数据
    history[index] = *data;
    index = (index + 1) % FILTER_SIZE;
    
    // 计算平均值
    MPU6050_Data sum = {0};
    for(int i = 0; i < FILTER_SIZE; i++) {
        sum.accel_x += history[i].accel_x;
        sum.accel_y += history[i].accel_y;
        sum.accel_z += history[i].accel_z;
        sum.gyro_x += history[i].gyro_x;
        sum.gyro_y += history[i].gyro_y;
        sum.gyro_z += history[i].gyro_z;
    }
    
    data->accel_x = sum.accel_x / FILTER_SIZE;
    data->accel_y = sum.accel_y / FILTER_SIZE;
    data->accel_z = sum.accel_z / FILTER_SIZE;
    data->gyro_x = sum.gyro_x / FILTER_SIZE;
    data->gyro_y = sum.gyro_y / FILTER_SIZE;
    data->gyro_z = sum.gyro_z / FILTER_SIZE;
}

/*

这个实现考虑了跌倒的主要特征：冲击、姿态变化和最终姿态。你可以根据实际应用场景调整参数和添加更多功能。
*/

